// #####################################
// File     : motor.c
// Author   : Chris Meaclem, Chris McPherson, Isaac, Andrew
// Date     : 28/07/2011
// Release  : Intermediate
// Purpose  : Library to provide control for motors via motor driver
// Notes    : 
// #####################################

/*Includes:*/
#include "motor.h"

/*END - Includes:*/
/*Definitions:*/
#define uint unsigned integer


//#####Need to specify port numbers:
#define MOTOR_RIGHT_FOR_PIO PIO_DEFINE(PB3, 1)
#define MOTOR_RIGHT_REV_PIO PIO_DEFINE(PB4, 1)
#define MOTOR_RIGHT_EN_PIO PIO_DEFINE(PB5, 1)

#define MOTOR_LEFT_FOR_PIO PIO_DEFINE(PB1, 1)
#define MOTOR_LEFT_REV_PIO PIO_DEFINE(PB2, 1)
#define MOTOR_LEFT_EN_PIO PIO_DEFINE(PB0, 1)

/** Define PIO pins driving LED matrix columns.  */
static const pio_t motorLeft[] = { MOTOR_LEFT_FOR_PIO, MOTOR_LEFT_REV_PIO, MOTOR_LEFT_EN_PIO };
static const pio_t motorRight[] = { MOTOR_RIGHT_FOR_PIO, MOTOR_RIGHT_REV_PIO, MOTOR_RIGHT_EN_PIO };

enum pioArrayPstns {pinFor = 0, pinRev, pinEn};

typedef struct {uint pinEn, uint pinFor, uint pinRev} motor_t;//Define a satatype for the motor object
enum motionStates {motion_Stop = 0, motion_For, motion_Left, motion_Right, motion_Rev};/*Define the motion states that the vechile can be in*/
enum motorStates {motor_Stop = 0, motor_For, motor_Rev};/*States the motor can be in*/
enum motors {motor_LeftID = 0, motor_rightID}; // May not need this, just to distinguish the motors apart. Use #define instead?

/*END - Definitions:*/
/*Globals:*/
motor_t motorLeft, motorRight;

/*END - Globals:*/


//public void motor_Init(uint leftEn, uint leftFor, uint leftRev, uint rightEn, uint rightFor, uint rightRev)
public void motor_Init(void)
{
	/* Description:
	* Initilise the motors
	* Scope: Global.
	* Input: left and right indicate what wheel. En=Enable pin, For=Forwards pin, Rev=Reverse Pin.
	*/
	
	
	//USE THIS METHOD WHEN PASSING PORT NUMBERS:
	/*Set output states and save info to motorLeft and motorRight*/
	/*
	motorLeft.pinEn = leftEn;
	motorLeft.pinFor = leftFor;
	motorLeft.pinRev = leftRev;
	
	motorRight.pinEn = rightEn;
	motorRight.pinFor = rightFor;
	motorRight.pinRev = rightRev;*/
	
	//uSE THIS METHOD WHEN USING DEFINED PIN NUMBERS
	//configure PIO as an initially LOW output.
	pio_config_set (MOTOR_RIGHT_FOR_PIO, PIO_OUTPUT_LOW);
	pio_config_set (MOTOR_RIGHT_REV_PIO, PIO_OUTPUT_LOW);
	pio_config_set (MOTOR_RIGHT_EN_PIO, PIO_OUTPUT_LOW);
	
	pio_config_set (MOTOR_LEFT_FOR_PIO, PIO_OUTPUT_LOW);
	pio_config_set (MOTOR_LEFT_REV_PIO, PIO_OUTPUT_LOW);
	pio_config_set (MOTOR_LEFT_EN_PIO, PIO_OUTPUT_LOW);
}

public void motor_Drive(uint state)
{
	/* Description:
	* function to execute desired motor actions. Uses 'motor_Set to actuate the desired state
	* Scope: Global.
	* Input: 'state' belongs to 'motionStates'
	*/
	switch(state)
	{
		case(motion_Stop):
			motor_Set(motor_LeftID, motor_Stop);
			motor_Set(motor_RightID, motor_Stop);
			break;
			
		case(motion_For):
			motor_Set(motor_LeftID, motor_For);
			motor_Set(motor_RightID, motor_For);
			break;
			
		case(motion_Left):
			motor_Set(motor_LeftID, motor_For);
			motor_Set(motor_RightID, motor_Stop);
			break;
			
		case(motion_Right):
			motor_Set(motor_LeftID, motor_Stop);
			motor_Set(motor_RightID, motor_For);
			break;
			
		case(motion_Rev):
			motor_Set(motor_LeftID, motor_Rev);
			motor_Set(motor_RightID, motor_Rev);
			break;
		default:
			//Error case, do something here to alert
			break;
	}
}

static void motor_Set(uint motorID, uint state)
{
	/* Description:
	* function to execute desired motor actions specifed
	* Scope: Internal.
	* Input: 'motorID' indicates the left or right motor
	* Input: 'state' indicates the state to set the motor
	*/
	
	/*Set output's here using HAL*/
	
	switch(state)
	{
		case(motor_Stop):
				if(motorID == motor_LeftID)
				{
					pio_output_low (MOTOR_LEFT_EN_PIO);
					pio_output_low (MOTOR_LEFT_FOR_PIO);
					pio_output_low (MOTOR_LEFT_REV_PIO);
				}
				else if(motorID == motor_RightID)
				{		
					pio_output_low (MOTOR_RIGHT_EN_PIO);
					pio_output_low (MOTOR_RIGHT_FOR_PIO);
					pio_output_low (MOTOR_RIGHT_REV_PIO);
				}
			break;
			
		case(motor_For):
				if(motorID == motor_LeftID)
				{
					pio_output_high (MOTOR_LEFT_FOR_PIO);
					pio_output_low (MOTOR_LEFT_REV_PIO);
					pio_output_high (MOTOR_LEFT_EN_PIO);
				}
				else if(motorID == motor_RightID)
				{				
					pio_output_high (MOTOR_RIGHT_FOR_PIO);
					pio_output_low (MOTOR_RIGHT_REV_PIO);
					pio_output_high (MOTOR_RIGHT_EN_PIO);
				}
			break;
				
		case(motor_Rev):
				if(motorID == motor_LeftID)
				{
					pio_output_low (MOTOR_LEFT_FOR_PIO);
					pio_output_high (MOTOR_LEFT_REV_PIO);
					pio_output_high (MOTOR_LEFT_EN_PIO);
				}
				else if(motorID == motor_RightID)
				{				
					pio_output_low (MOTOR_RIGHT_FOR_PIO);
					pio_output_high (MOTOR_RIGHT_REV_PIO);
					pio_output_high (MOTOR_RIGHT_EN_PIO);
				}
			break;
		default:
			//Error case, do something here to alert
			break;
	}
	
}


